#include "motor_cybergear.h"

void motor_enable(uint8_t id)
{
    uint32_t can_id = id_convert(HEADER_ENABLE, id);
    uint8_t TxData[8] = {0x00};
    CAN_Send(can_id, TxData);
}

void motor_disable(uint8_t id)
{
    uint32_t can_id = id_convert(HEADER_DISABLE, id);
    uint8_t TxData[8] = {0x00};
    CAN_Send(can_id, TxData);
}

void motor_set_zero(uint8_t id)
{
    uint32_t can_id = id_convert(HEADER_SET_ZERO, id);
    uint8_t TxData[8] = {0x01};
    CAN_Send(can_id, TxData);
}

void motor_control_mit(uint8_t id, MotorMIT_Typedef *motor_mit)
{
    uint32_t can_id = id_convert_mit(
        HEADER_CONTROL_MIT, mit_torque_to_u16(motor_mit->forward_torque), id);
    static uint8_t TxData[8];
    static uint16_t pos, vel, kp, ki;
    pos = mit_pos_to_u16(motor_mit->target_position);
    vel = mit_pos_to_u16(motor_mit->target_velocity);
    kp = mit_pos_to_u16(motor_mit->kp);
    ki = mit_pos_to_u16(motor_mit->ki);
    TxData[0] = u16_to_u8h(pos);
    TxData[1] = u16_to_u8l(pos);
    TxData[2] = u16_to_u8h(vel);
    TxData[3] = u16_to_u8l(vel);
    TxData[4] = u16_to_u8h(kp);
    TxData[5] = u16_to_u8l(kp);
    TxData[6] = u16_to_u8h(ki);
    TxData[7] = u16_to_u8l(ki);
    CAN_Send(can_id, TxData);
}

MotorStatus_Typedef motor_get_status(uint32_t identifier, const uint8_t *data)
{
    MotorStatus_Typedef motor_status;
    motor_status.id = identifier & 0xff;
    motor_status.error_info = (identifier >> 16) & 0x3f;
    motor_status.mode_status = (identifier >> 22) & 0x03;
    motor_status.actual_position = u16_to_pos(u8_to_u16(data[0], data[1]));
    motor_status.actual_velocity = u16_to_vel(u8_to_u16(data[2], data[3]));
    motor_status.actual_torque = u16_to_torque(u8_to_u16(data[4], data[5]));
    motor_status.actual_temperature =
        u16_to_temperature(u8_to_u16(data[6], data[7]));

    return motor_status;
}

void motor_set_mode(uint8_t id, uint8_t mode)
{
    uint32_t can_id = id_convert(HEADER_SET_CONTROL_CMD, id);
    uint8_t TxData[8] = {0x00};
    TxData[0] = u16_to_u8l(PARA_RUN_MODE);
    TxData[1] = u16_to_u8h(PARA_RUN_MODE);
    TxData[2] = 0;
    TxData[3] = 0;
    TxData[4] = mode;
    TxData[5] = 0;
    TxData[6] = 0;
    TxData[7] = 0;

    CAN_Send(can_id, TxData);
}

void motor_write_param(uint8_t id, uint16_t index, float data)
{
    uint32_t can_id = id_convert(HEADER_SET_PARAM, id);
    uint32_t data_int = *(uint32_t *)&data;
    uint8_t TxData[8];
    TxData[0] = u16_to_u8l(index);
    TxData[1] = u16_to_u8h(index);
    TxData[2] = 0x06;
    TxData[3] = 0;
    TxData[4] = u32_to_u8ll(data_int);
    TxData[5] = u32_to_u8lh(data_int);
    TxData[6] = u32_to_u8hl(data_int);
    TxData[7] = u32_to_u8hh(data_int);

    CAN_Send(can_id, TxData);
}

void motor_set_control_cmd(uint8_t id, uint16_t index, float data)
{
    uint32_t can_id = id_convert(HEADER_SET_CONTROL_CMD, id);
    uint32_t data_int = *(uint32_t *)&data;
    uint8_t TxData[8];
    TxData[0] = u16_to_u8l(index);
    TxData[1] = u16_to_u8h(index);
    TxData[2] = 0;
    TxData[3] = 0;
    TxData[4] = u32_to_u8ll(data_int);
    TxData[5] = u32_to_u8lh(data_int);
    TxData[6] = u32_to_u8hl(data_int);
    TxData[7] = u32_to_u8hh(data_int);

    CAN_Send(can_id, TxData);
}

void motor_set_current(uint8_t id, float iq)
{
    motor_set_control_cmd(id, PARA_IQ_REF, iq);
}

void motor_set_velocity(uint8_t id, float vel)
{
    motor_set_control_cmd(id, PARA_SPD_REF, vel);
}

void motor_set_position(uint8_t id, float pos)
{
    motor_set_control_cmd(id, PARA_LOC_REF, pos);
}

void motor_set_pid(uint8_t id, MotorPID_Typedef *motor_pid)
{
    motor_write_param(id, PARA_CUR_FILT_GAIN, motor_pid->cur_filt_gain);
    HAL_Delay(2);
    motor_write_param(id, PARA_CUR_KP, motor_pid->cur_kp);
    HAL_Delay(2);
    motor_write_param(id, PARA_CUR_KI, motor_pid->cur_ki);
    HAL_Delay(2);
    motor_write_param(id, PARA_SPD_KP, motor_pid->spd_kp);
    HAL_Delay(2);
    motor_write_param(id, PARA_SPD_KI, motor_pid->spd_ki);
    HAL_Delay(2);
    motor_write_param(id, PARA_LOC_KP, motor_pid->loc_kp);
    HAL_Delay(2);
    motor_write_param(id, PARA_SPD_FILT_GAIN, motor_pid->spd_filt_gain);
    HAL_Delay(2);
}
